/*
 * test_node.h
 *
 *  Created on: 16.09.2013
 *      Author: alex
 */
#ifndef TEST_NODE_H_
#define TEST_NODE_H_

#include <vector>
#include <bullet/LinearMath/btVector3.h>
#include <ros/node_handle.h>
#include <ros/timer.h>
#include <ros/ros.h>
#include <octomap_msgs/OctomapBinary.h>
#include <octomap/octomap.h>
#include <octomap_msgs/conversions.h>
//#include <traverse_analysis/traverse_analysis.h>

namespace bullet_rviz_test
{

class RvizMotionState;
class Heightfield;
typedef boost::shared_ptr<Heightfield> HeightfieldPtr;
class HeightfieldVisualization;
typedef boost::shared_ptr<HeightfieldVisualization> HeightfieldVisualizationPtr;
class Robot;
typedef boost::shared_ptr<Robot> RobotPtr;
class RobotVisualization;
typedef boost::shared_ptr<RobotVisualization> RobotVisualizationPtr;
class Physics;
typedef boost::shared_ptr<Physics> PhysicsPtr;
class PhysicalObject;

class StabilityChecker
{
public:
//  static int main(int argc, char ** argv);
  StabilityChecker(bool show_visualization_, bool load_from_file);

  bool check(double start_x, double start_y, double start_theta);

  void map_callback(const octomap_msgs::OctomapBinary::ConstPtr& msg);

  bool has_map();

  double max_tilt;
  double max_movement;

private:
  struct RobotInfo {
    RobotPtr robot_;
    RobotVisualizationPtr visualization_;
    btVector3 initial_position_;
  };
  typedef boost::shared_ptr<RobotInfo> RobotInfoPtr;


  bool show_visualization;
  octomap::OcTree* tree;
  ros::Time last_accepted_octomap_time;

  int run(double start_x, double start_y, double start_theta);
  void resetRobots(double robot_x, double robot_y, double robot_theta, double drop_z = -1000.0);
//  void computeCosts(RobotInfo * robot_info);
  void createHeightfieldFromOctomap();
//  void robotMotionCallback(RobotInfo * robot_info);

  ros::Subscriber sub;

  ros::NodeHandle nh_;
  ros::Timer reset_timer_;

  PhysicsPtr physics_;
  HeightfieldPtr heightfield_;
  HeightfieldVisualizationPtr heightfield_visualization_;
  std::vector<RobotInfoPtr> robots_;
};

}

#endif /* TEST_NODE_H_ */
